import re
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import interp1d

# ==================== 配置文件路径 ====================
log_path = r"C:\Users\LSQ_CX\Desktop\下午日志详细分析\control_node_3388_59803.log"

# 存储数据：格式为 [(timestamp, x, y, z), ...]
mydrone_data = []
estimate_data = []

# 正则表达式
# 匹配 cur_ekf_time:1757399209.3936532
pattern_time = re.compile(r"cur_ekf_time:\s*([-\d.eE]+)")

# 匹配 mydrone_enu:[73.25499559 30.64202854 17.41350524]
pattern_estimate = re.compile(r"estimate_info:\s*\[([-\d.eE+]+),\s*([-\d.eE+]+),\s*([-\d.eE+]+)")
pattern_mydrone = re.compile(r"mydrone_enu:\s*\[\s*([-\d.eE+-]+)\s+([-\d.eE+-]+)\s+([-\d.eE+-]+)")

# 当前行的时间（用于关联数据）
current_time = None

# ==================== 解析日志 ====================
with open(log_path, 'r', encoding='utf-8') as f:
    for line in f:
        # 更新当前时间戳
        time_match = pattern_time.search(line)
        if time_match:
            current_time = float(time_match.group(1))

        # 提取 mydrone_enu
        drone_match = pattern_mydrone.search(line)
        if drone_match and current_time is not None:
            x = float(drone_match.group(1))
            y = float(drone_match.group(2))
            z = float(drone_match.group(3))
            mydrone_data.append((current_time, x, y, z))

        # 提取 estimate_info 前三项
        est_match = pattern_estimate.search(line)
        if est_match and current_time is not None:
            x = float(est_match.group(1))
            y = float(est_match.group(2))
            z = float(est_match.group(3))
            estimate_data.append((current_time, x, y, z))

# 转为 numpy 数组
mydrone_data = np.array(mydrone_data)  # shape: (N, 4)
estimate_data = np.array(estimate_data)  # shape: (M, 4)

print(f"提取到 mydrone_enu 数据点: {len(mydrone_data)}")
print(f"提取到 estimate_info 数据点: {len(estimate_data)}")

if len(mydrone_data) == 0 or len(estimate_data) == 0:
    print("数据不足，无法计算距离。")
else:
    # ==================== 时间对齐：统一时间轴 ====================
    # 取两个数据流共同的时间区间
    t_min = max(mydrone_data[0, 0], estimate_data[0, 0])
    t_max = min(mydrone_data[-1, 0], estimate_data[-1, 0])

    # 筛选出在公共区间内的数据
    mydrone_valid = mydrone_data[(mydrone_data[:, 0] >= t_min) & (mydrone_data[:, 0] <= t_max)]
    estimate_valid = estimate_data[(estimate_data[:, 0] >= t_min) & (estimate_data[:, 0] <= t_max)]

    # 如果没有交集
    if len(mydrone_valid) == 0 or len(estimate_valid) == 0:
        print("两个数据流无时间交集，无法对齐。")
    else:
        # 创建插值函数
        # mydrone 插值函数
        f_mydrone_x = interp1d(mydrone_valid[:, 0], mydrone_valid[:, 1], kind='linear', fill_value="extrapolate")
        f_mydrone_y = interp1d(mydrone_valid[:, 0], mydrone_valid[:, 2], kind='linear', fill_value="extrapolate")
        f_mydrone_z = interp1d(mydrone_valid[:, 0], mydrone_valid[:, 3], kind='linear', fill_value="extrapolate")

        # estimate 插值函数
        f_est_x = interp1d(estimate_valid[:, 0], estimate_valid[:, 1], kind='linear', fill_value="extrapolate")
        f_est_y = interp1d(estimate_valid[:, 0], estimate_valid[:, 2], kind='linear', fill_value="extrapolate")
        f_est_z = interp1d(estimate_valid[:, 0], estimate_valid[:, 3], kind='linear', fill_value="extrapolate")

        # 统一采样时间点（使用更密集的估计数据时间点）
        t_common = np.sort(np.unique(np.concatenate([mydrone_valid[:, 0], estimate_valid[:, 0]])))

        # 插值得到同步位置
        mydrone_interp = np.array([
            f_mydrone_x(t_common),
            f_mydrone_y(t_common),
            f_mydrone_z(t_common)
        ]).T  # shape: (T, 3)

        estimate_interp = np.array([
            f_est_x(t_common),
            f_est_y(t_common),
            f_est_z(t_common)
        ]).T  # shape: (T, 3)

        # ==================== 计算实时距离 ====================
        distances = np.linalg.norm(mydrone_interp - estimate_interp, axis=1)  # 欧氏距离

        # ==================== 统计信息 ====================
        max_dist = np.max(distances)
        min_dist = np.min(distances)
        avg_dist = np.mean(distances)
        std_dist = np.std(distances)
        max_idx = np.argmax(distances)
        min_idx = np.argmin(distances)

        print(f"最大距离: {max_dist:.3f} m")
        print(f"最小距离: {min_dist:.3f} m")
        print(f"平均距离: {avg_dist:.3f} m (±{std_dist:.3f})")

        # ==================== 绘图 ====================
        plt.figure(figsize=(12, 6))
        plt.plot(t_common - t_common[0], distances, color='purple', linewidth=1.5, label='Real-time Distance')

        # 标注最远和最近点
        plt.scatter(t_common[max_idx] - t_common[0], max_dist, color='red', s=60, label=f'Max: {max_dist:.2f}m', zorder=5)
        plt.scatter(t_common[min_idx] - t_common[0], min_dist, color='blue', s=60, label=f'Min: {min_dist:.2f}m', zorder=5)

        # 填充背景（可选）
        plt.fill_between(t_common - t_common[0], distances, alpha=0.3, color='purple')

        # 设置标签
        plt.xlabel("Time (s) from start")
        plt.ylabel("Distance (m)")
        plt.title("Real-time Distance Between My Drone and Estimated Target")
        plt.grid(True, linestyle='--', alpha=0.6)
        plt.legend()

        # 添加文本统计
        stats_text = f"Mean: {avg_dist:.2f} m\nStd: {std_dist:.2f} m"
        plt.text(0.02, 0.98, stats_text, transform=plt.gca().transAxes,
                 fontsize=10, verticalalignment='top',
                 bbox=dict(boxstyle="round", facecolor="wheat", alpha=0.8))

        plt.tight_layout()
        plt.show()